/***************************************************************************\
 * Copyright (C) by Keio University
 * OpenNISensorDriver.cpp created in 07 2012.
 * Mail : fdesorbi@hvrl.ics.keio.ac.jp
 *
 * OpenNISensorDriver.cpp is part of HVRL Engine Library.
 *
 * The HVRL Engine Library is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * The HVRL Engine Library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 \***************************************************************************/

#include "hvrl/sensor/OpenNISensorDriver.hpp"
#include "hvrl/common/Common.hpp"

#if (XN_PLATFORM == XN_PLATFORM_LINUX_X86 || XN_PLATFORM == XN_PLATFORM_LINUX_ARM)
#define UNIX
#define GLX_GLXEXT_LEGACY
#endif

#if (XN_PLATFORM == XN_PLATFORM_MACOSX)
#define MACOS
#endif

#include <XnLog.h>
#include <XnTypes.h>

namespace hvrl {

OpenNISensorDriver::OpenNISensorDriver(void) :
		numberofdevices(0), initialized(false) {

	/*
	 * Initialization of the logging system based on the hvrl's one
	 */
	xnLogSetConsoleOutput(false);
	xnLogSetMaskMinSeverity("OpenNIDepthInputStream", XN_LOG_SEVERITY_NONE);
	if (hvrl::Log::getInstance().getLevel() == hvrl::COMPLETE
			|| hvrl::Log::getInstance().getLevel() == hvrl::DEBUG_MODE) {
		xnLogSetConsoleOutput(true);
		xnLogSetMaskMinSeverity("OpenNIDepthInputStream", XN_LOG_VERBOSE);
	} else {
		if (hvrl::Log::getInstance().getLevel() == hvrl::WARNING_MODE) {
			xnLogSetFileOutput(true);
			xnLogSetMaskMinSeverity("OpenNIDepthInputStream", XN_LOG_WARNING);
		} else {
			if (hvrl::Log::getInstance().getLevel() == hvrl::ERROR_MODE) {
				xnLogSetFileOutput(true);
				xnLogSetMaskMinSeverity("OpenNIDepthInputStream", XN_LOG_ERROR);
			}
		}
	}
	xnLogInitSystem();

	if (this->refresh() == false) {
		throw hvrl::Exception("OpenNISensorDriver::OpenNISensorDriver",
				"Failed to initialize the driver");
	}
}

OpenNISensorDriver::~OpenNISensorDriver(void) {

}

bool OpenNISensorDriver::refresh(void) {

	this->numberofdevices = 0;

	XnStatus nRetVal = XN_STATUS_OK;

	if (this->initialized == false) {
		/*
		 * Create the context for OpenNI
		 */
		nRetVal = this->context.Init();
		if (nRetVal != ((XnStatus) 0)) {
			hvrl::Log::add().error("OpenNISensorDriver::init",
					xnGetStatusString(nRetVal));
			return false;
		}

		/*
		 * Add the license key to the context
		 */
		XnLicense m_license;
		strcpy(m_license.strVendor, "PrimeSense");
		strcpy(m_license.strKey, "0KOIk2JeIBYClPWVnMoRKn5cdY4=");
		this->context.AddLicense(m_license);
		this->initialized = true;
	}

	/*
	 * Get the list of the devices
	 */
	xn::NodeInfoList list;
	xn::EnumerationErrors errors;
	nRetVal = this->context.EnumerateProductionTrees(XN_NODE_TYPE_DEVICE, NULL,
			list, &errors);
	if (nRetVal != ((XnStatus) 0)) {
		hvrl::Log::add().error("OpenNISensorDriver::init",
				xnGetStatusString(nRetVal));
		return false;
	}

	this->deviceslist.clear();

	/*
	 * Count the number of available OpenNI devices
	 */
	unsigned int i = 0;
	for (xn::NodeInfoList::Iterator it = list.Begin(); it != list.End();
			++it, ++i) {
		this->numberofdevices++;
		xn::NodeInfo deviceNodeInfo = *it;
		const XnProductionNodeDescription& description =
				deviceNodeInfo.GetDescription();
		this->deviceslist.push_back(std::string(description.strName) + " ["
						+ hvrl::numberToString(i).c_str() + "]");
		this->drivername = std::string(description.strVendor);
	}

	return true;
}

}
